// RigCalib.cpp : implementation file
//

#include "stdafx.h"
#include "manualCalib.h"
#include "RigCalib.h"
#include "GrabList.h"
#include "CCamera.h"
#include "CalibData.h"
#include "cvxIo.h"

// CRigCalib dialog

IMPLEMENT_DYNAMIC(CRigCalib, CDialog)

CRigCalib::CRigCalib(CWnd* pParent /*=NULL*/)
	: CDialog(CRigCalib::IDD, pParent)
	, m_pParent(pParent)
{

}

CRigCalib::~CRigCalib()
{
}

void CRigCalib::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	DDX_Control(pDX, IDC_LEFT_FIX_PRINCIPAL_POINT, m_leftFixPP);
	DDX_Control(pDX, IDC_LEFT_ZERO_TANGENT_DIST, m_leftZeroTanDist);
	DDX_Control(pDX, IDC_LEFT_RADIAL_DIST, m_leftRadialDist);
	DDX_Control(pDX, IDC_LEFT_CALIB_RST, m_leftCalibRst);
	DDX_Control(pDX, IDC_RIGHT_FIX_PRINCIPAL_POINT, m_rightFixPP);
	DDX_Control(pDX, IDC_RIGHT_ZERO_TAN_DIST, m_rightZeroTanDist);
	DDX_Control(pDX, IDC_RIGHT_RADIAL_DIST, m_rightRadialDist);
	DDX_Control(pDX, IDC_RIGHT_CALIB_RST, m_rightCalibRst);
	DDX_Control(pDX, IDC_FIX_ASPECT_RATIO_LEFT, m_leftFixAspectRatio);
	DDX_Control(pDX, IDC_FIX_ASPECT_RATIO_RIGHT, m_rightFixAspectRatio);
	DDX_Control(pDX, IDC_RIG_CALIB_RIG_ORI, m_rigOrientation);
	DDX_Control(pDX, IDC_RIG_CALIB_EIGHT_PTS, m_rigOriEightPts);
	DDX_Control(pDX, IDC_ONLY_CALIB_LEFT, m_calibLeftOnly);
	DDX_Control(pDX, IDC_BUNDLE_ADJUST, m_isBoudleAdjust);
	DDX_Control(pDX, IDC_SHOW_ERR_DISTRIBUTION, m_isShowErrDistribution);
}


BEGIN_MESSAGE_MAP(CRigCalib, CDialog)
	ON_BN_CLICKED(IDOK, &CRigCalib::OnBnClickedOk)
	ON_BN_CLICKED(IDCANCEL, &CRigCalib::OnBnClickedCancel)
	ON_BN_CLICKED(IDC_CALIB, &CRigCalib::OnBnClickedCalib)
	ON_EN_CHANGE(IDC_LEFT_CALIB_RST, &CRigCalib::OnEnChangeLeftCalibRst)
END_MESSAGE_MAP()


// CRigCalib message handlers

void CRigCalib::OnBnClickedOk()
{
	// TODO: Add your control notification handler code here
	if (UpdateData(true))
	{
		m_pParent->UpdateData(false);
		DestroyWindow();
	}
	//OnOK();
}

void CRigCalib::OnBnClickedCancel()
{
	// TODO: Add your control notification handler code here
	DestroyWindow();
	//OnCancel();
}

void CRigCalib::PostNcDestroy()
{
	// TODO: Add your specialized code here and/or call the base class

	CDialog::PostNcDestroy();

	if(m_pParent)
	{		
		((CGrabList*)m_pParent)->m_rigCalib = NULL;		
	}
	delete this;
}

BOOL CRigCalib::OnInitDialog()
{
	CDialog::OnInitDialog();

	// TODO:  Add extra initialization here
	//init left camera property
	m_leftFixPP.SetCheck(BST_UNCHECKED);
	m_leftZeroTanDist.SetCheck(BST_UNCHECKED);
	m_leftFixAspectRatio.SetCheck(BST_UNCHECKED);
	m_leftRadialDist.AddString("K2");
	m_leftRadialDist.AddString("K3");
	m_leftRadialDist.AddString("K4");
	m_leftRadialDist.SetCurSel(0);
	m_leftCalibRst.SetWindowText("Left camera not calibrated!\r\n");

	//init right camera property
	m_rightFixPP.SetCheck(BST_UNCHECKED);
	m_rightZeroTanDist.SetCheck(BST_UNCHECKED);
	m_rightFixAspectRatio.SetCheck(BST_UNCHECKED);
	m_rightRadialDist.AddString("K2");
	m_rightRadialDist.AddString("K3");
	m_rightRadialDist.AddString("K4");
	m_rightRadialDist.SetCurSel(0);
	m_rightCalibRst.SetWindowText("Right camera not calibrated!\r\n");

	//init rig orientation
	m_rigOrientation.SetWindowText("Camera rig not calibrated!\r\n");
	m_rigOriEightPts.SetCheck(BST_UNCHECKED);
	m_calibLeftOnly.SetCheck(BST_CHECKED);
	m_isBoudleAdjust.SetCheck(BST_UNCHECKED);
	m_isShowErrDistribution.SetCheck(BST_UNCHECKED);

	return TRUE;  // return TRUE unless you set the focus to a control
	// EXCEPTION: OCX Property Pages should return FALSE
}

void CRigCalib::OnBnClickedCalib()
{
	// TODO: Add your control notification handler code here

	if (m_pParent)
	{
		if (m_calibLeftOnly.GetCheck() == BST_CHECKED)
		{
			CCamera leftCamera;
			CvSize imageSize = cvSize(1920, 1080);

			//corners
			CList<CGrabItem*> *pList = &(((CGrabList*)m_pParent)->m_itemList);
			POSITION aPos = pList->GetHeadPosition();
			while(aPos)
			{
				CGrabItem *item = pList->GetNext(aPos);
				if (!item->m_leftPts.empty())
				{
					leftCamera.addFeature2d(item->m_leftPts);
					imageSize = cvGetSize(item->m_leftImage);
				}
			}

			//flags
			int leftFlag = 0;
			int rightFlag = 0;
			GetCalibrationFlags(leftFlag, rightFlag);			
			leftCamera.setFlags(leftFlag);			

			//image size
			leftCamera.setImageSize(imageSize);
			vector<Point3f> pts3D;
			if ((((CGrabList*)m_pParent)->m_board))
			{
				 (((CGrabList*)m_pParent)->m_board)->Get3Dpts(pts3D);
				 leftCamera.setCorners3d(pts3D);
			}

			double tt = clock();

			bool okLeft  = leftCamera.calib(leftFlag, true);

			if (okLeft)
			{
				//@todo
				if (m_isBoudleAdjust.GetCheck() == BST_CHECKED)
				{
					if ((((CGrabList*)m_pParent)->m_board))
					{
						leftCamera.bundleAdjust3DLinear((((CGrabList*)m_pParent)->m_board)->GetUnitLength(),
							(((CGrabList*)m_pParent)->m_board)->GetPatternSize());
						okLeft = leftCamera.calib(leftFlag | CV_CALIB_USE_INTRINSIC_GUESS, true);
					}		
				}
				double cost_t = clock() - tt;
				{
					FILE *tPf = fopen("calib_time.txt", "w");
					fprintf(tPf, "%f\n", cost_t);
					fclose(tPf);
				}				

				char bufLeft[1024] = {NULL};
				CString preText;
				leftCamera.GetIntrinsicParameter(bufLeft);					
				m_leftCalibRst.GetWindowText(preText);
				m_leftCalibRst.SetWindowText(preText + CString(bufLeft));
				if (m_pParent)
				{
					vector<float> emptyVec;
					((CGrabList*)m_pParent)->updateReprojectionError(leftCamera.getReprojectionError(),
						emptyVec);

					//update extrinsic parameter in grab list
					if ((((CGrabList*)m_pParent)->m_board))
					{
						float uLength = (((CGrabList*)m_pParent)->m_board)->GetUnitLength();
						CvSize size   = (((CGrabList*)m_pParent)->m_board)->GetPatternSize();
						leftCamera.computerEulerAngle(uLength, size.width, size.height);
						leftCamera.computerNormal(uLength, size);
						((CGrabList*)m_pParent)->updateOrientation(leftCamera.m_thetaVec, 
							leftCamera.m_phiVec, leftCamera.m_zDistanceVec, 
							leftCamera.m_rodriguesAngle, leftCamera.m_planeNormal, true);
						if (m_isShowErrDistribution.GetCheck() == BST_CHECKED)
						{
							leftCamera.visualizeRPR((((CGrabList*)m_pParent)->m_board)->GetPatternSize(), 40);
						}							
					}

				}
				//save camera parameters
				leftCamera.saveIntrinsic("left_intrinsic.txt");	
				leftCamera.saveExtrinsic("left_extrinsic.txt");				
			}
			else
			{
				AfxMessageBox(_T("Left Camera calibration failed!"), MB_OK);
			}			
		}
		else
		{
			CCamera leftCamera;
			CCamera rightCamera;
			CvSize imageSize = cvSize(1920, 1080);

			//corners
			CList<CGrabItem*> *pList = &(((CGrabList*)m_pParent)->m_itemList);
			POSITION aPos = pList->GetHeadPosition();
			while(aPos)
			{
				CGrabItem *item = pList->GetNext(aPos);
				if (!item->m_leftPts.empty() && !item->m_rightPts.empty())
				{
					leftCamera.addFeature2d(item->m_leftPts);
					rightCamera.addFeature2d(item->m_rightPts);
					imageSize = cvGetSize(item->m_leftImage);
				}
			}

			//flags
			int leftFlag = 0;
			int rightFlag = 0;
			GetCalibrationFlags(leftFlag, rightFlag);	

			leftCamera.setFlags(leftFlag);
			rightCamera.setFlags(rightFlag);

			//image size
			leftCamera.setImageSize(imageSize);
			rightCamera.setImageSize(imageSize);

			vector<Point3f> pts3D;
			if ((((CGrabList*)m_pParent)->m_board))
			{
				(((CGrabList*)m_pParent)->m_board)->Get3Dpts(pts3D);
				leftCamera.setCorners3d(pts3D);
				rightCamera.setCorners3d(pts3D);
			}

			bool okLeft  = leftCamera.calib(leftFlag, true);
			bool okRight = rightCamera.calib(rightFlag, true);

			if (okLeft && okRight)
			{
				char bufLeft[1024] = {NULL};
				char bufRight[1024] = {NULL};
				char bufRig[1024] = {NULL};

				CString preText;
				leftCamera.GetIntrinsicParameter(bufLeft);					
				m_leftCalibRst.GetWindowText(preText);
				m_leftCalibRst.SetWindowText(preText + CString(bufLeft));

				preText = CString("");
				rightCamera.GetIntrinsicParameter(bufRight);
				m_rightCalibRst.GetWindowText(preText);
				m_rightCalibRst.SetWindowText(preText + CString(bufRight));

				if (m_pParent)
				{
					((CGrabList*)m_pParent)->updateReprojectionError(leftCamera.getReprojectionError(),
						rightCamera.getReprojectionError());
				}

				//get camera rig orientation

				leftCamera.GetOrientation(rightCamera, m_rigOriEightPts.GetCheck() == BST_CHECKED, bufRig);
				preText = CString("");
				m_rigOrientation.GetWindowText(preText);
				m_rigOrientation.SetWindowText(preText + CString(bufRig));

				//save camera parameters
				leftCamera.saveIntrinsic("left_intrinsic.txt");
				rightCamera.saveIntrinsic("right_intrinsic.txt");

			}
			else
			{
				AfxMessageBox(_T("Stereo Camera calibration failed!"), MB_OK);
			}

		}		
	}
}

void CRigCalib::GetCalibrationFlags(int &leftFlag, int &rightFlag)
{
	leftFlag = 0;
	if (m_leftFixPP.GetCheck() == BST_CHECKED)
	{
		leftFlag |= CV_CALIB_FIX_PRINCIPAL_POINT;
	}
	if (m_leftZeroTanDist.GetCheck() == BST_CHECKED)
	{
		leftFlag |= CV_CALIB_ZERO_TANGENT_DIST;
	}
	if (m_leftFixAspectRatio.GetCheck() == BST_CHECKED)
	{
		leftFlag |= CV_CALIB_FIX_ASPECT_RATIO;
	}

	if (m_leftRadialDist.GetCurSel() == 0)
	{
		leftFlag |= CV_CALIB_FIX_K2|CV_CALIB_FIX_K3|CV_CALIB_FIX_K4;
	}
	else if (m_leftRadialDist.GetCurSel() == 1)
	{
		leftFlag |= CV_CALIB_FIX_K3|CV_CALIB_FIX_K4;
	}
	else if (m_leftRadialDist.GetCurSel() == 2)
	{
		leftFlag |= CV_CALIB_FIX_K4;
	}
	rightFlag = 0;
	
	if (m_rightFixPP.GetCheck() == BST_CHECKED)
	{
		rightFlag |= CV_CALIB_FIX_PRINCIPAL_POINT;
	}
	if (m_rightZeroTanDist.GetCheck() == BST_CHECKED)
	{
		rightFlag |= CV_CALIB_ZERO_TANGENT_DIST;
	}
	if (m_rightFixAspectRatio.GetCheck() == BST_CHECKED)
	{
		rightFlag |= CV_CALIB_FIX_ASPECT_RATIO;
	}
	if (m_rightRadialDist.GetCurSel() == 0)
	{
		rightFlag |= CV_CALIB_FIX_K2|CV_CALIB_FIX_K3|CV_CALIB_FIX_K4;
	}
	else if (m_rightRadialDist.GetCurSel() == 1)
	{
		rightFlag |= CV_CALIB_FIX_K3|CV_CALIB_FIX_K4;
	}
	else if (m_rightRadialDist.GetCurSel() == 2)
	{
		rightFlag |= CV_CALIB_FIX_K4;
	}
}

void CRigCalib::OnEnChangeLeftCalibRst()
{
	// TODO:  If this is a RICHEDIT control, the control will not
	// send this notification unless you override the CDialog::OnInitDialog()
	// function and call CRichEditCtrl().SetEventMask()
	// with the ENM_CHANGE flag ORed into the mask.

	// TODO:  Add your control notification handler code here
}
